#include "device.h"

//简介：M6020电机设置电压函数
void m6020SetVoltage(P_M6020_CTRL pctrl, CAN_HandleTypeDef *hcan)
{
    uint8_t i;
	static uint8_t m6020_p1data[8];
    static uint32_t m6020_txmailbox1;

    static CAN_TxHeaderTypeDef M6020_PACK1_HDR =
    {
        0x1FF,
        0,
        CAN_ID_STD,
        CAN_RTR_DATA,
        8,
        DISABLE
    };
    
    if(pctrl->pack == M6020_PACK_1_4)
    {
         for(i=0;i <= 3;i++)
        {
            m6020_p1data[i *2] = (pctrl ->setvol[i]>>8)&0xff;
            m6020_p1data[i *2 +1] = pctrl -> setvol[i] &0x00ff;
        }
        HAL_CAN_AddTxMessage(hcan, &M6020_PACK1_HDR, m6020_p1data, &m6020_txmailbox1);
    }
}

//简介：M6020反馈值复位
void m6020ResetFeedBack(P_M6020_FDBK pfdbk)
{
    P_M6020_FDBK_UNIT pfirst = &pfdbk->m1;
    int i;
    for(i=0;i<=7;i++)
    {
        pfirst[i].fdbktime      = 0;
        pfirst[i].angle         = 0;
        pfirst[i].voltage       = 0;
        pfirst[i].count         = 0;
        pfirst[i].speed         = 0;
    }
}

//简介：M6020电机反馈信息整理
uint8_t m6020FeedBack(P_CAN_RX pcanrx,P_M6020_FDBK pfdbk)
{
    uint16_t id = pcanrx->hdr.StdId - 0x204;
//	  if(pcanrx->hdr.StdId == 0x205)  id=2  ;
//	   else id=1; 
	  P_M6020_FDBK_UNIT pfirst = &pfdbk->m1;
    pfirst[id - 1].speed_count = 0;
    pfirst[id - 1].speed = (pcanrx->data[2] << 8) + pcanrx->data[3];//转速
    pfirst[id - 1].prelas_angle = pfirst[id - 1].last_angle;
    pfirst[id - 1].last_angle =  pfirst[id - 1].angle;
    pfirst[id - 1].fdbktime = 0;
    pfirst[id - 1].angle =(pcanrx->data[0] << 8) + pcanrx->data[1];
    pfirst[id - 1].voltage = (pcanrx->data[4] << 8) + pcanrx->data[5];  //实际转矩电流 
    return id;
}

